﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;


namespace Kaleta.Xna.Extensions
{
    public static class Vector2Extensions
    {
        /// <summary>
        /// Gets a normalized (Length=1) 2D vector created from this angle.
        /// </summary>
        /// <param name="radians"></param>
        /// <returns></returns>
        static public Vector2 GetNormalizedVector2(this float radians)
        {
            return new Vector2()
            {
                X = (float)Math.Cos(radians - Math.PI),
                Y = (float)Math.Sin(radians - Math.PI)
            };
        }

        /// <summary>
        /// Gets an angle in radians between this vector and the other one. 
        /// </summary>
        /// <param name="source"></param>
        /// <param name="another"></param>
        /// <returns></returns>
        static public float GetAngle(this Vector2 source, Vector2 another)
        {
            return (float)Math.Atan2(source.Y - another.Y, source.X - another.X);
        }


        /// <summary>
        /// Gets an angle in radians between this vector and Vector(1,0).
        /// </summary>
        /// <param name="source"></param>
        /// <returns></returns>
        static public float GetAngle(this Vector2 source)
        {
            return (float)Math.Atan2(source.Y - 0, source.X - 1);
        }


        static public Vector2 Clamp(this Vector2 source, float maxLength)
        {
            if (source.Length() > maxLength)
            {
                source.Normalize();
                return source * maxLength;
            }
            else return source;
        }

        /// <summary>
        /// Gets the intercepting vector from this position, moving at source velocity, to specified target position, which moves at target velocity. 
        /// The magnitude (length) of this intercepting vector is specified by interceptVelocityMagnitude.
        /// </summary>
        /// <param name="sourcePosition"></param>
        /// <param name="targetPosition"></param>
        /// <param name="sourceVelocity"></param>
        /// <param name="targetVelocity"></param>
        /// <param name="interceptVelocityMagnitude"></param>
        /// <returns></returns>
        static public Vector2 GetInterceptVector(this Vector2 sourcePosition, Vector2 targetPosition, Vector2 sourceVelocity, Vector2 targetVelocity, float interceptVelocityMagnitude)
        {
            Vector2 dist = targetPosition - sourcePosition;
            Vector2 interceptVelocity;

            float t;
            float d = dist.Length();
            float velocitiesLength = (targetVelocity - sourceVelocity).Length();

            //parameters:
            int maxTries = 100;
            float maxRelativeError = 1.05f;
            float incRelativeM = -0.015f;


            float maxError = interceptVelocityMagnitude * (maxRelativeError - 1);
            float incM = incRelativeM * interceptVelocityMagnitude;
            float finalM = interceptVelocityMagnitude;
            int tries = maxTries;

            do
            {
                t = d / (interceptVelocityMagnitude + velocitiesLength);
                interceptVelocity = dist / t + targetVelocity - sourceVelocity;
                interceptVelocityMagnitude += incM; tries--;
            }
            while (Math.Abs(interceptVelocity.Length() - finalM) > maxError && tries > 0);

            //DebuggerView.SetValue("Last shot tries", (maxTries - tries).ToString());
            //DebuggerView.SetValue("Last shot velocity", (interceptVelocity).Length());

            //DebuggerView.SetValue("Shot ALL shots", DebuggerView.GetValue<int>("Shot ALL shots") + 1);
            //DebuggerView.SetValue("Shot ALL tries", DebuggerView.GetValue<int>("Shot ALL tries") + maxTries - tries);


            //DebuggerView.SetValue("Shot AVG tries", DebuggerView.GetValue<int>("Shot ALL tries") / DebuggerView.GetValue<int>("Shot ALL shots"));

            return interceptVelocity + sourceVelocity;
        }

        public static Vector2 GetInterceptVector2(this Vector2 shooter, Vector2 shooterVelocity, float bulletSpeed,Vector2 target, Vector2 targetVelocity)
        {


            float p1x = shooter.X;
            float p1y = shooter.Y;

            float p2x = target.X;
            float p2y = target.Y;

            float v2x = targetVelocity.X - shooterVelocity.X;
            float v2y = targetVelocity.Y - shooterVelocity.Y;

            float a = (v2x * v2x + v2y * v2y - bulletSpeed * bulletSpeed);
            float b = (2 * p2x * v2x - 2 * p1x * v2x + 2 * p2y * v2y - 2 * p1y * v2y);
            float c = (p2x * p2x - 2 * p2x * p1x + p1x * p1x + p2y * p2y - 2 * p2y * p1y + p1y * p1y);

            float result = (-b - (float)Math.Sqrt(b * b - 4 * a * c)) / (2 * a);

            // guard for not a number
            if (Single.IsNaN(result))
            {
                return target;
            }

            return target + (result * targetVelocity);
        }
    }
}
